/**
 * @file cx_task.c
 * @author Char_X (2421498578@qq.com)
 * @brief a RTOS task schedualer imcompletement
 * @version 0.1
 * @date 2024-06-15
 * 
 * @copyright Copyright (c) 2024
 * 
 */
#include "cx_task.h"
TCB_t *CurrentTask;
__attribute__((used)) static void TaskIdleHandler(void *null);

static TCB_t IdleTask;
#define TaskGroupNum 5

StackType_t IdleTaskStack[TaskMinimalStack];

cx_slist_t ReadyTaskGroup[TaskGroupNum+1] = 
{
	{.head = CX_NULL,.tail=CX_NULL,},
	{.head = CX_NULL,.tail=CX_NULL,},
	{.head = CX_NULL,.tail=CX_NULL,},
	{.head = CX_NULL,.tail=CX_NULL,},
	{.head = CX_NULL,.tail=CX_NULL,},
	{.head = &(IdleTask.owner),.tail=&(IdleTask.owner)}
};

cx_slist_t BlockTaskList=
{
	.head = CX_NULL,
	.tail = CX_NULL
};

cx_slist_t DelayList=
{
	.head = CX_NULL,
	.tail = CX_NULL
};

static void TaskExitErr(void)
{
	for(;;);
}
/**
 * @brief function TaskSwitchGetNext
 * 
 * @return TCB_t* Next task block *
 */

TCB_t *TaskSwitchGetNext(void)
{
	cx_slist_t *check_list;
	cx_slist_item_t *ret_item = &(IdleTask.owner);
	for(int i=0; i<TaskGroupNum+1;i++)
	{
		check_list = &(ReadyTaskGroup[i]);
		if(check_list->head != CX_NULL)
		{
			ret_item = cx_slist_sort_first_to_end(check_list);
			break;
		}
	}
	return (TCB_t*)(ret_item->content);
}

/**
 * @brief Context switch
 * 
 */
void TaskSwitchContext(void)
{
	// if(CurrentTask == &Task1)
	// {
	// 	CurrentTask = &Task2;
	// }
	// else
	// {
	// 	CurrentTask = &Task1;
	// }
	CurrentTask = TaskSwitchGetNext();
}

#define COMPILER_GCC
#ifndef COMPILER_GCC
__asm void StartFirstTask(void)
{
		PRESERVE8

    ldr r0, =0xE000ED08
		ldr r0, [r0]
		ldr r0, [r0]


		msr msp, r0

		cpsie i
		cpsie f
    dsb
    isb

    svc 0
    nop
    nop
}

__asm void SVC_Handler(void)
{
	extern CurrentTask;
	PRESERVE8
	ldr r3, =CurrentTask
	ldr r1, [r3]
	ldr r0, [r1]
	ldmia r0!, {r4-r11}
	msr psp,r0
	isb
	mov r0,#0
	msr basepri,r0
	orr r14,#0xd
	bx r14
}

__asm void PendSV_Handler(void)
{
	extern CurrentTask;
	extern TaskSwitchContext;
	
	PRESERVE8
	
	mrs r0, psp
	isb
	ldr r3, =CurrentTask
	ldr r2,[r3]
	stmdb r0!,{r4-r11}
	str r0,[r2]
	stmdb sp!,{r3,r14}
	mov r0, #191
	msr basepri, r0
	dsb
	isb
	bl TaskSwitchContext
	mov r0, #0
	msr basepri, r0
	ldmia sp!, {r3, r14}
	ldr r1, [r3]
	ldr r0, [r1]
	ldmia r0!, {r4-r11}
	msr psp, r0
	isb
	bx r14
	nop
}
#else
void StartFirstTask(void) __attribute__ ((naked, aligned(8)));


// void SVC_Handler(void)
// {
//     asm volatile (
//         "   ldr r3, =CurrentTask         \n" // Load address of CurrentTask into r3
//         "   ldr r1, [r3]                 \n" // Load value of CurrentTask (current task control block pointer) into r1
//         "   ldr r0, [r1]                 \n" // Load process stack pointer from the current task control block
//         "   ldmia r0!, {r4-r11}          \n" // Restore r4-r11 from the process stack
//         "   msr psp, r0                  \n" // Update PSP with the new process stack pointer
//         "   isb                          \n" // Instruction Synchronization Barrier
//         "   mov r0, #0                   \n" // Clear base priority mask
//         "   msr basepri, r0              \n" // Set base priority register to 0 to enable all interrupts
//         "   orr r14, #0xd                \n" // Modify the EXC_RETURN value to return to Thread mode with PSP
//         "   bx r14                       \n" // Branch to the return address (exit the handler)
//         "   nop                          \n" // No operation (in case of alignment issues)
//     );
// }

void StartFirstTask(void)
{
    asm volatile (

        "ldr r0, =0xE000ED08\n" // Set interrupt vector
		"ldr r0, [r0]\n"
		"ldr r0, [r0]\n"


		"msr msp, r0\n"

		"cpsie i\n"
		"cpsie f\n"
        "dsb\n"
        "isb\n"

        "svc 0\n"
        "nop\n"
        "nop\n"
    );
		
}

void TaskYild(void)
{
	portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT;
    // asm(
    //     // :"__dsb"( portSY_FULL_READ_WRITE )
	//     // :"__isb"( portSY_FULL_READ_WRITE )
    // );
    // __dsb()
    asm(
        "dsb\n"
        "isb\n"
    );
	
}
// void PendSV_Handler(void)
// {
//     asm volatile (
//         "   mrs r0, psp                  \n" // Move PSP (Process Stack Pointer) to r0
//         "   isb                          \n" // Instruction Synchronization Barrier
//         "   ldr r3, =CurrentTask         \n" // Load address of CurrentTask into r3
//         "   ldr r2, [r3]                 \n" // Load value of CurrentTask (current task control block pointer) into r2
//         "   stmdb r0!, {r4-r11}          \n" // Store r4-r11 on the process stack
//         "   str r0, [r2]                 \n" // Save the new process stack pointer into the current task control block
//         "   stmdb sp!, {r3, r14}         \n" // Store r3 (CurrentTask) and r14 (return address) on the main stack
//         "   mov r0, #191                 \n" // Set base priority mask to 191 (0xBF) to disable interrupts with priority lower or equal to 191
//         "   msr basepri, r0              \n" // Set base priority register
//         "   dsb                          \n" // Data Synchronization Barrier
//         "   isb                          \n" // Instruction Synchronization Barrier
//         "   bl TaskSwitchContext         \n" // Call TaskSwitchContext function
//         "   mov r0, #0                   \n" // Clear base priority mask
//         "   msr basepri, r0              \n" // Set base priority register to 0 to enable all interrupts
//         "   ldmia sp!, {r3, r14}         \n" // Restore r3 (CurrentTask) and r14 (return address) from the main stack
//         "   ldr r1, [r3]                 \n" // Load value of CurrentTask (new task control block pointer) into r1
//         "   ldr r0, [r1]                 \n" // Load new process stack pointer from the new task control block
//         "   ldmia r0!, {r4-r11}          \n" // Restore r4-r11 from the new process stack
//         "   msr psp, r0                  \n" // Update PSP with the new process stack pointer
//         "   isb                          \n" // Instruction Synchronization Barrier
//         "   bx r14                       \n" // Branch to the return address (exit the handler)
//         "   nop                          \n" // No operation (in case of alignment issues)
//     );
// }

#endif

StackType_t* TaskStackInit(StackType_t *TopOfStack,void *entry, void *arg)
{
	TopOfStack--;
	*TopOfStack = INITAL_XPSR;
	TopOfStack--;
	*TopOfStack = ((StackType_t)entry)&ENTRY_ADDRESS_MASK;
	TopOfStack--;
	*TopOfStack = (StackType_t)TaskExitErr;
	TopOfStack-=5;
	*TopOfStack = (StackType_t)arg;
	TopOfStack -= 8;
	return TopOfStack;
/*******************************************/
//	xPSR    = INITAL_XPSR                  |
//  r15     = entry & ENTRY_ADDRESS_MASK   |
//  r14(LR) = TaskExitErr                  v
//  r12     = 0                           low
//  r3      = 0
//  r2      = 0
//  r1      = 0
//  r0      = arg
// ^ Auto saved
// v Manual saved
//  r11     = 0
//  r10     = 0
//  r9      = 0
//  r8      = 0
//  r7      = 0
//  r6      = 0
//  r5      = 0
//  r4      = 0
//  TopOfStack  <--
/*******************************************/
}	

task_err_t TaskInitialize(TCB_t *tcb,void* ProgEntry, void* StackBuffer, unsigned int StackDepth,int prio)
{
	task_err_t ret;
	tcb->pStack = StackBuffer;
	tcb->ProgEntry = ProgEntry;
	tcb->StackOfTop = tcb->pStack+StackDepth-1;
	tcb->StackOfTop = (StackType_t*)((cx_uint32_t)(tcb->StackOfTop) &(~((cx_uint32_t)(0x07))));
	tcb->StackOfTop = TaskStackInit((StackType_t*)(tcb->StackOfTop),ProgEntry,(void*)0);
	tcb->owner.content = (void*)tcb;
	if(prio < TaskGroupNum)
	{
		tcb->priority = prio;
		ret = TASK_ERR_OK;
	}
	else
	{
		ret = TASK_ERR_INITIALIZE_PRIORITY_EXCEED;
	}
	return ret;
}


void StartSchedual(void)
{
	SYSTICK_CONFIG();
	TaskInitialize(&IdleTask,TaskIdleHandler,IdleTaskStack,TaskMinimalStack,TaskGroupNum);
	CurrentTask = TaskSwitchGetNext();
	portNVIC_SYSPRI3_REG |= portNVIC_PENDSV_PRI;
	portNVIC_SYSPRI3_REG |= portNVIC_SYSTICK_PRI;
	StartFirstTask();
}


task_err_t TaskCreat(TCB_t *tcb,void* ProgEntry, void* StackBuffer, unsigned int StackDepth,int prio)
{
	task_err_t ret;
	ret = TaskInitialize(tcb,ProgEntry,StackBuffer,StackDepth,prio);
	tcb->eta_delay_ticks = 0;
	if(ret == TASK_ERR_OK)
	{
		cx_slist_append(&(ReadyTaskGroup[tcb->priority]),&(tcb->owner));
	}
	return ret;

}

__attribute__((used)) void TaskIdleHandler(void *null)
{
	while(1);
}

void TaskDelay(unsigned long long ticks)
{
	EntryCritical();
	CurrentTask->eta_delay_ticks = ticks;
	cx_slist_item_delete(&(ReadyTaskGroup[CurrentTask->priority]),&(CurrentTask->owner));
	cx_slist_append(&DelayList,&(CurrentTask->owner));
	ExitCritical();
	TaskYild();
}

#define portSysTick_Handler SysTick_Handler

void portSysTick_Handler(void)
{
	cx_uint32_t ulBasepri;
	cx_slist_item_t *check = DelayList.head;
	ulBasepri = EntryCriticalFromISR();
	system_ticks++;
	while(check!=CX_NULL)
	{
		((TCB_t*)(check->content))->eta_delay_ticks--;
		if(((TCB_t*)(check->content))->eta_delay_ticks == 0)
		{
			cx_slist_item_delete(&DelayList,check);
			cx_slist_append(&(ReadyTaskGroup[((TCB_t*)(check->content))->priority]),check);
		}
		check=check->next;
	}
	ExitCriticalFromISR(ulBasepri);
	TaskYild();
}